#include "RoboterRaupe.h"
#include "Tempomat.h"


void TempomatInit(void){


}

void SetRpmSpeed(int Seite, int Rpm){
	RpmSoll[Seite]=Rpm;
}

void SetRpm(unsigned int Seite){
	static int Bremse;
	static int aktPos;
	int richtung;
	long abweichung;
	long neuePos;
	
	if (Bremse > 0 && Bremse < 3){
		SetServo(Seite, -500);
		aktPos=-500;
		PORTD=0;
		Bremse++;
		return;
	}
	if (Bremse==3)
	{
		SetServo(Seite, 0);
		aktPos=0;
		Bremse=0;
		PORTD=0xFF;
		return;
	}
	//Bremse=0;
	//PORTD=0xFF;
	if (RpmSoll[Seite] < 10 && RpmSoll[Seite] > -10){
		SetServo(Seite, 0);
		aktPos=0;
		return;
	}
	richtung=1;
	//aktPos=GetServoPos(Seite);
	if (RpmSoll[Seite] < 0){
		richtung = -1;
		//Rpm*=-1;
	}
	abweichung=(richtung*RpmSoll[Seite])-RpmIst(Seite);
	


	if (aktPos>0 && richtung ==-1){
		SetServo(Seite, 0);
		Bremse=1;
		aktPos=0;
		return;
	}

	if (aktPos<0 && richtung ==1){
		SetServo(Seite, 0);
		aktPos=0;
		return;
	}
		
	if (abweichung < 5 && abweichung > -5)
		return;

	neuePos=richtung*(abweichung/18); //((long)Rpm/80L);

	//if (abweichung > 0)
		neuePos=((neuePos)*(neuePos)*(neuePos))/1000;
	//else
	//	neuePos=(abweichung*abweichung)/(long)-1000;
	
	if (neuePos > 50) neuePos=50;
	if (neuePos < -50) neuePos=-50;

	neuePos+=aktPos;

	if (neuePos > 500) neuePos = 500;
	
	if (neuePos < -500) neuePos = -500;

	if ((richtung==1) && (neuePos < 0)) neuePos=100;
	if ((richtung==-1) && (neuePos > 0)) neuePos=-100;

	SetServo(Seite, neuePos);
	aktPos=neuePos;
	return;

	
}
